//Geprogrammeerd door Joris Goes, team 4
import lejos.nxt.*;


public class MowUnitController {
	private boolean isDown = false;
	private int lowPoint = 0;
	private boolean isCalibrated = false;
	private TouchSensor t = new TouchSensor(Configuration.TOUCHSENSOR_PORT);
	private Motor motor = Configuration.MOTOR_MOWUNIT;

	public void up() {
		if (isDown) {
			motor.setSpeed(40);
			motor.rotateTo(0);
			isDown = false;
		}
	}

	public void down() {
		if (!isDown) {
			motor.setSpeed(40);
			motor.rotateTo(lowPoint);
			isDown = true;
		}
	}

	public boolean getIsDown() {
		return isDown;
	}

	public void calibrate() {
		if(isCalibrated == false) {
			motor.setSpeed(30);
			
			//Maaikop achterwaards naar boven en 1 seconde wachten.
			while (!t.isPressed()) { motor.backward(); }
			motor.stop();
			
			//Motor voorwaards naar beneden bewegen zodat de touchsensor niet meer wordt ingedrukt.
			while (t.isPressed() == true) { motor.forward(); }
			motor.resetTachoCount();
	
			while (t.isPressed() == false) { motor.forward(); }
			System.out.println(motor.getTachoCount());
			int h2 = motor.getTachoCount();
			
			motor.stop();
			lowPoint = h2/2;
			motor.rotateTo(0, true);
			
			System.out.println(lowPoint);
			isCalibrated = true;
		}
		else { System.out.println("De maaiunit is al gekalibreerd."); }
	}
	
	public boolean getIsCalibrated() {
		return isCalibrated;
	}
}
